#include "drv8301.h"
#include "bsp/bsp_drivers.h"

static bool read_reg(const RegName_e regName, uint16_t* data);
static bool write_reg(const RegName_e regName, const uint16_t data);

static SPI_InitTypeDef spi_config_ = {
    .Mode = SPI_MODE_MASTER,
    .Direction = SPI_DIRECTION_2LINES,
    .DataSize = SPI_DATASIZE_16BIT,
    .CLKPolarity = SPI_POLARITY_LOW,
    .CLKPhase = SPI_PHASE_2EDGE,
    .NSS = SPI_NSS_SOFT,
    .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16,
    .FirstBit = SPI_FIRSTBIT_MSB,
    .TIMode = SPI_TIMODE_DISABLE,
    .CRCCalculation = SPI_CRCCALCULATION_DISABLE,
    .CRCPolynomial = 10,
};

bool drv8301_init(float requested_gain) {
    uint16_t gain_setting = 3;
	uint16_t val;
    float gain_choices[] = {10.0f, 20.0f, 40.0f, 80.0f};
    while (gain_setting && (gain_choices[gain_setting] > requested_gain)) {
        gain_setting--;
    }
    struct RegisterFile new_config;

    new_config.control_register_1 =
          (21 << 6) // Overcurrent set to approximately 150A at 100degC. This may need tweaking.
        | (0b01 << 4) // OCP_MODE: latch shut down
        | (0b0 << 3) // 6x PWM mode
        | (0b0 << 2) // don't reset latched faults
        | (0b00 << 0); // gate-drive peak current: 1.7A

    new_config.control_register_2 =
          (0b0 << 6) // OC_TOFF: cycle by cycle
        | (0b00 << 4) // calibration off (normal operation)
        | (gain_setting << 2) // select gain
        | (0b00 << 0); // report both over temperature and over current on nOCTW pin
    gpio_write(DRV8301_nCS_PIN, true);
	gpio_write(DRV8301_ENA_PIN, false);
	gpio_init(DRV8301_nCS_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_FREQ_LOW);
	gpio_init(DRV8301_ENA_PIN, GPIO_MODE_OUTPUT_PP, GPIO_PULLUP, GPIO_SPEED_FREQ_LOW);
	cpu_udelay(20);
	gpio_write(DRV8301_ENA_PIN, true);
	cpu_mdelay(10);

    // Write current configuration
    bool wrote_regs = write_reg(kRegNameControl1, new_config.control_register_1)
                       && write_reg(kRegNameControl1, new_config.control_register_1)
                       && write_reg(kRegNameControl1, new_config.control_register_1)
                       && write_reg(kRegNameControl1, new_config.control_register_1)
                       && write_reg(kRegNameControl1, new_config.control_register_1) // the write operation tends to be ignored if only done once (not sure why)
                       && write_reg(kRegNameControl2, new_config.control_register_2);
    if (!wrote_regs) {
        return false;
    }

    // Wait for configuration to be applied
    cpu_udelay(100);

    bool is_read_regs = read_reg(kRegNameControl1, &val) && (val == new_config.control_register_1)
                      && read_reg(kRegNameControl2, &val) && (val == new_config.control_register_2);
    if (!is_read_regs) {
        return false;
    }
	exti_subscribe(DRV8301_FAULT_PIN, EXTI_EDGE_FALL, DRV8301_Fault_IrqHandler);
	return true;
}


static bool read_reg(const RegName_e regName, uint16_t* data) {
    u16 tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Read, regName, 0);
    if (!spi_transfer(&spi_config_, DRV8301_nCS_PIN, (uint8_t *)(&tx_buf_), NULL, 1, 10)) {
        return false;
    }
    
    cpu_udelay(1);

    tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Read, regName, 0);
    u16 rx_buf_ = 0xffff;
    if (!spi_transfer(&spi_config_, DRV8301_nCS_PIN, (uint8_t *)(&tx_buf_), (uint8_t *)(&rx_buf_), 1, 10)) {
        return false;
    }

    cpu_udelay(1);

    if (rx_buf_ == 0xbeef) {
        return false;
    }

    if (data) {
        *data = rx_buf_ & 0x07FF;
    }
    
    return true;
}

static bool write_reg(const RegName_e regName, const uint16_t data) {
    // Do blocking write
    u16 tx_buf_ = build_ctrl_word(DRV8301_CtrlMode_Write, regName, data);
    if (!spi_transfer(&spi_config_, DRV8301_nCS_PIN, (uint8_t *)(&tx_buf_), NULL, 1, 10)) {
        return false;
    }
    cpu_udelay(1);
    return true;
}

